/***************************************************************************
 *   Copyright (C) 2010 by Oleg Goncharov  *
 *   $EMAIL$                           *                          
 *                                                                         *
 *   This file is part of ChessVision.                                     *
 *                                                                         *
 *   ChessVision is free software; you can redistribute it and/or modify   *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include <iostream>
#include <cmath>

#include <common/MathHelperFunctions.h>

#include "vision/geomtypes.h"
#include "katanafuncs.h"

namespace Katana {

	int FindObjectInPlane(CKatanaSensing& katana, double h, int steps, int polltime, int waittime) {
		SActuatorPosition p, m;
		double dh = h/steps;
		double r;
		const TMotPVP * pvp = katana.GetMotor(M_BASE)->GetPVP();
		int max_val = 0, sens_value;
		double angle;
		short old_speed = katana.getMotorVelocityLimit(M_BASE);
		katana.setMotorVelocityLimit(M_BASE, 10);
	
		katana.getCoordinates (p.x, p.y, p.z, p.phi, p.theta, p.psi);
		r = sqrt(p.x*p.x + p.y*p.y);
		angle = asin(h/r);
	
		katana.moveMotorTo (M_GRP, 0, true, waittime);
	
		h /= 2.0; r -= h;
		angle /= 2.0; 
		KNI::Timer poll(polltime);
		
		for(int i  = 0; i < steps; i++, r += dh) {
			std::cout << "STEP" << std::endl;
			katana.moveRobotTo(r*cos(p.psi+angle), r*sin(p.psi+angle), p.z, 0.0, 0.0, 0.0, true, waittime);
			std::cout << "ROT" << std::endl;
			katana.incDegrees(M_BASE, KNI_MHF::rad2deg(-2.0*angle), false);
			do {
				poll.Start();
				poll.WaitUntilElapsed();
			
				katana.UpdateSensors();
				sens_value = katana.GetRawSensorValue(S_DOWN_LEFT) + katana.GetRawSensorValue(S_DOWN_RIGHT);
			
				if (sens_value > max_val) {
					max_val = sens_value;
					katana.getCoordinates (m.x, m.y, m.z, m.phi, m.theta, m.psi);
					std::cout << "val = " << sens_value << std::endl;
					std::cout << "max = " << m.x << " " << m.y << " " << m.z << std::endl;
				}

			} while (! katana.MotOnTarget(M_BASE));
		
			angle = -angle;
		}
		katana.setMotorVelocityLimit(M_BASE, old_speed);
	
		katana.moveRobotTo(m.x, m.y, m.z, 0.0, 0.0, 0.0, true, waittime);
	
		return max_val / 2;
	}

	int GrabDotObject(CikBase& katana, double depth, double open, int waittime) {
		SActuatorPosition p;
	
		katana.movDegrees(Katana::M_GRP, open, true, 100 ,waittime);
		katana.getCoordinates (p.x, p.y, p.z, p.phi, p.theta, p.psi);
		katana.moveRobotTo(p.x, p.y, p.z - depth, p.phi, p.theta, p.psi, true, waittime);
	
		katana.movDegrees(Katana::M_GRP, 5, true, 100, waittime);
		return 0;
	}

	int GrabVolObject(CKatanaSensing& katana, double depth, double open, double deg_force, int polltime, int waittime) {
		SActuatorPosition p;
		double p1, p2, d1, d2;
		KNI::Timer poll(polltime);
		KNI::Timer t(waittime); 
	
		katana.movDegrees(Katana::M_GRP, open, true, 100, waittime);
	//std::cout << katana.IsObstacle(S_IN_RIGHT_FRONT) << katana.IsObstacle(S_IN_LEFT_FRONT) << std::endl;
		katana.getCoordinates (p.x, p.y, p.z, p.phi, p.theta, p.psi);
		double r = sqrt(p.x*p.x + p.y*p.y);
	// коррекция на открытие захвата
		r = (r +100.0*(1.0 - cos(KNI_MHF::deg2rad(open/2.0))))/r;
		p.x *= r; p.y *= r;
	 
		katana.moveRobotTo(p.x, p.y, p.z - depth, p.phi, p.theta, p.psi, true, waittime);
	
		katana.movDegrees(Katana::M_GRP, deg_force);
		t.Start();
		do {
			poll.Start();
			poll.WaitUntilElapsed();
			katana.UpdateSensors();
		
			if (t.Elapsed()) throw MotorTimeoutException();
		
			d1 = katana.GetMax()[S_IN_LEFT_FRONT] - katana.GetSensorValue(S_IN_LEFT_FRONT);
			d2 = katana.GetMax()[S_IN_RIGHT_FRONT] - katana.GetSensorValue(S_IN_RIGHT_FRONT);
		
		//std::cout << "max = " << d1 << " " << d2  << " " << katana.IsObstacle(S_IN_RIGHT_FRONT) << katana.IsObstacle(S_IN_LEFT_FRONT) << std::endl;
		} while ( (d1 > 30 || d2 > 30) && !katana.MotOnTarget(M_GRP));
	
		katana.decDegrees(Katana::M_GRP, deg_force, true, 100, waittime);
		return 0;
	}

	int GrabCrash(CKatanaSensing& katana, double depth, double open, int waittime) {
		SActuatorPosition p;
	
		katana.movDegrees(Katana::M_GRP, open, true, 100, waittime);
	//std::cout << katana.IsObstacle(S_IN_RIGHT_FRONT) << katana.IsObstacle(S_IN_LEFT_FRONT) << std::endl;
		katana.getCoordinates (p.x, p.y, p.z, p.phi, p.theta, p.psi);
		double r = sqrt(p.x*p.x + p.y*p.y);
	// коррекция на открытие захвата
		r = (r +100.0*(1.0 - cos(KNI_MHF::deg2rad(open/2.0))))/r;
		p.x *= r; p.y *= r;
	 
		katana.moveRobotTo(p.x, p.y, p.z - depth, p.phi, p.theta, p.psi, true, waittime);
	
		try {
			katana.movDegrees(Katana::M_GRP, 5, true);
		} catch (MotorCrashException& e) {
			katana.unBlock();
			katana.incDegrees(Katana::M_GRP, 5, true);
		}
		return 0;
	}

	int GrabForce(CKatanaSensing& katana, double depth, double open, int polltime, int waittime) {
		SActuatorPosition p;
		KNI::Timer poll(polltime);
		KNI::Timer t(waittime);
	
		katana.movDegrees(Katana::M_GRP, open, true, 100, waittime);
		katana.getCoordinates (p.x, p.y, p.z, p.phi, p.theta, p.psi);
		double r = sqrt(p.x*p.x + p.y*p.y);
	// коррекция на открытие захвата
		r = (r +100.0*(1.0 - cos(KNI_MHF::deg2rad(open/2.0))))/r;
		p.x *= r; p.y *= r;
	 
		katana.moveRobotTo(p.x, p.y, p.z - depth, p.phi, p.theta, p.psi, true, waittime);
	
		katana.movDegrees(Katana::M_GRP, 5, false);
		t.Start();
		do {
			poll.Start();
			poll.WaitUntilElapsed();
			katana.UpdateSensors();
		
			if (t.Elapsed()) throw MotorTimeoutException();
		
		//std::cout << katana.IsObstacle(S_IN_RIGHT_FRONT_FORCE) << katana.IsObstacle(S_IN_LEFT_FRONT_FORCE) << std::endl;
		
		//d1 = katana.GetMax()[S_IN_LEFT_FRONT] - katana.GetSensorValue(S_IN_LEFT_FRONT);
		//d2 = katana.GetMax()[S_IN_RIGHT_FRONT] - katana.GetSensorValue(S_IN_RIGHT_FRONT);
		
		//std::cout << "max = " << d1 << " " << d2  << " " << katana.IsObstacle(S_IN_RIGHT_FRONT) << katana.IsObstacle(S_IN_LEFT_FRONT) << std::endl;
		} while (! katana.IsObstacle(S_IN_RIGHT_FRONT_FORCE) && ! katana.IsObstacle(S_IN_LEFT_FRONT_FORCE) );
		return 0;
	}

	double FindObjectUnderGripper(CKatanaSensing& katana, const cv::Point3d& up, double depth, int polltime, int waittime) {
		katana.moveRobotTo(up.x, up.y, up.z, 0.0, 0.0, 0.0, true, waittime);
	
		katana.moveRobotTo(up.x, up.y, up.z - depth, 0.0, 0.0, 0.0);
		KNI::Timer poll(polltime);
		KNI::Timer t(waittime); 
	
		t.Start();
		do {
			poll.Start();
			poll.WaitUntilElapsed();
			if (t.Elapsed()) throw MotorTimeoutException();
			katana.UpdateSensors();
		
		//std::cout << "obs = " << katana.IsObstacle(S_DOWN_RIGHT) << katana.IsObstacle(S_DOWN_LEFT) << std::endl;
		} while (!katana.IsObstacle(S_DOWN_LEFT) && !katana.IsObstacle(S_DOWN_RIGHT) && !katana.RobotOnTarget());
	
		SActuatorPosition p;
		katana.getCoordinates (p.x, p.y, p.z, p.phi, p.theta, p.psi);
		katana.moveRobotTo(up.x, up.y, p.z, 0.0, 0.0, 0.0, true, waittime);
	
		return p.z - up.z + depth;
	}
	
	void PseudoContinousControl(CikBase& katana, const Path3D& path, double ref_shift, int polltime, int waittime) {
		KNI::Timer poll(polltime);
		KNI::Timer timer(waittime);
		SActuatorPosition pos;
		
		timer.Start();
		for(Path3D::const_iterator target = path.begin(); target != path.end(); target++) {
			katana.moveRobotTo(target->x, target->y, target->z, 0.0, 0.0, 0.0, false);
			do {
				poll.Start();
				poll.WaitUntilElapsed();
				if (timer.Elapsed()) throw MotorTimeoutException();
			
				katana.getCoordinates(pos.x, pos.y, pos.z, pos.psi, pos.theta, pos.phi);
			
			} while ( cv::norm(*target - Point3d(pos.x, pos.y, pos.z)) > ref_shift );
		}
		katana.moveRobotTo(path.back().x, path.back().y, path.back().z, 0.0, 0.0, 0.0, true, waittime - timer.ElapsedTime());
	}
	
	void PseudoLinearMove(CikBase& katana, const Point3d& to, double segment_len, double ref_shift, int polltime, int waittime) {
		SActuatorPosition pos;
		katana.getCoordinates(pos.x, pos.y, pos.z, pos.psi, pos.theta, pos.phi);
		Point3d from(pos.x, pos.y, pos.z);
		Path3D path;
		Point3d shift;
		double len = cv::norm(to - from);
		segment_len = MIN(segment_len, 2.0*ref_shift);
		int nsegments = cv::saturate_cast<int>(len / segment_len);
		shift = (1.0/double(nsegments))*(cv::Point3d(CvPoint3D32f(to - from)));
		
		path.push_back(from);
		for(int i = 0; i < nsegments; i++) {
			path.push_back(path.back() + shift);
		}
		
		PseudoContinousControl(katana, path, ref_shift, polltime, waittime);
	}

}

